/**
 ****************************************************************************************************
 * @file        pid.c
 * @author      ForTomorrow
 * @version     V1.0
 * @date        2024-07-16
 * @brief       简易 PID
 ****************************************************************************************************
 * @attention
 * P  产生向目标点运动的主要动力
 * I  消除静差
 * D  减少过冲产生的振荡
 ****************************************************************************************************
 */

#include "MOTOR/pid.h"

/**
  * @brief  创建一个新的 PID
  * @param  Kp    比例
  * @param  Ki i  积分
  * @param  Kd    微分
  * @retval 无
  */
void PID_Init(PID_HandleTypeDef *hpid, float Kp, float Ki, float Kd, float dt)
{
  hpid->Kp = Kp;
  hpid->Ki = Ki;
  hpid->Kd = Kd;
  hpid->dt = dt;
  
  hpid->Imax    = 2e9;
  hpid->err     = 0;
  hpid->err_pre = 0;
  hpid->err_sum = 0;
  hpid->err_dif = 0;
}


/**
  * @brief  计算 PID 值
  * @param  hpid    PID 句柄
  * @param  err     误差值
  * @retval PID 值
  */
float PID_Base_Cale(PID_HandleTypeDef *hpid, float err)
{
  hpid->err = err;
  hpid->err_sum += hpid->err * hpid->dt;
  
  PID_Limit(&hpid->err_sum, -hpid->Imax, hpid->Imax);

  hpid->err_dif = (hpid->err-hpid->err_pre) / hpid->dt;
  hpid->err_pre = hpid->err;

  return hpid->Kp*hpid->err + hpid->Ki*hpid->err_sum + hpid->Kd*hpid->err_dif;
}


/**
  * @brief  计算 PD 值
  * @param  hpid    PID 句柄
  * @param  err     误差值
  * @retval PD 值
  */
float PD_Base_Cale(PID_HandleTypeDef *hpid, float err)
{
  hpid->err = err;
  hpid->err_dif = (hpid->err-hpid->err_pre) / hpid->dt;
  hpid->err_pre = hpid->err;

  return hpid->Kp*hpid->err + hpid->Kd*hpid->err_dif;
}


/**
  * @brief  计算 PI 值
  * @param  hpid     PID 句柄
  * @param  err      误差值
  * @retval PD 值
  */
float PI_Base_Cale(PID_HandleTypeDef *hpid, float err)
{
  hpid->err = err;
  hpid->err_sum += hpid->err * hpid->dt;
  
  PID_Limit(&hpid->err_sum, -hpid->Imax, hpid->Imax);

  return hpid->Kp*hpid->err + hpid->Ki*hpid->err_sum;
}


/**
  * @brief  计算 P 值
  * @param  hpid    PID 句柄
  * @param  err     误差值
  * @retval P 计算值
  */
float P_Base_Cale(PID_HandleTypeDef *hpid, float err)
{
  hpid->err = err;
  return hpid->Kp*hpid->err;
}


/**
  * @brief  计算 I 值
  * @param  hpid    PID 句柄
  * @param  err     误差值
  * @retval I 计算值
  */
float I_Base_Cale(PID_HandleTypeDef *hpid, float err)
{
  hpid->err_sum += err * hpid->dt;
  PID_Limit(&hpid->err_sum, -hpid->Imax, hpid->Imax);
  return hpid->Ki*hpid->err_sum;
}


/**
  * @brief  计算 D 值
  * @param  hpid    PID 句柄
  * @param  err     误差值
  * @retval D 计算值
  */
float D_Base_Cale(PID_HandleTypeDef *hpid, float err)
{
  hpid->err_dif = (err-hpid->err_pre) / hpid->dt;
  hpid->err_pre = err;
  return hpid->Kd*hpid->err_dif;
}


/**
  * @brief  计算带有一阶滤波器（First-order Filter）的 P 值
  * @param  hpid         PID 句柄
  * @param  err          误差值
  * @param  filter_coef  过滤器系数 (0.0-1.0) 系数值越大则当前误差值权重越大
  * @retval P 计算值
  */
float P_Filter_FO(PID_HandleTypeDef *hpid, float err, float filter_coef)
{
  hpid->err = filter_coef*err + (1-filter_coef)*hpid->err_pre;
  hpid->err_pre = hpid->err;
  return hpid->Kp*hpid->err;
}


/**
 ****************************************************************************************************
 * 辅助函数区
 ****************************************************************************************************
 */

/**
  * @brief  限幅
  * @param  val     限制的对象
  * @param  vmin    不能超过的最小值
  * @param  vmax    不能超过的最大值
  * @retval 无
  */
void PID_Limit(float *val, float vmin, float vmax)
{
  if (*val > vmax) *val = vmax;
  else if (*val < vmin) *val = vmin;
}


/**
  * @brief  计算 二次函数 ax^2 + bx + c 值
  * @param  x    自变量值
  * @param  a    二次系数
  * @param  b    一次系数
  * @param  c    常数系数
  * @retval 函数值
  */
float PID_Quadratic_Cale(float x, float a, float b, float c)
{
  return a*x*x + b*x + c+x;
}


/**
 * @brief  映射函数
 * @param  x          输入值
 * @param  in_min     输入值最小值
 * @param  in_max     输入值最大值
 * @param  out_min    输出值最小值
 * @param  out_max    输出值最大值
 * @retval 映射后的值
 */
float PID_Mapping(float x, float in_min, float in_max, float out_min, float out_max)
{
  return (x-in_min) * (out_max-out_min) / (in_max-in_min) + out_min;
}
